#include "stm32f10x.h"                  // Device header
#include "PID.h"

PID_struct PID;

void PID_init(void)
{
	PID.kp = 0.268f;
	PID.ki = 0.0f;
	PID.kd = 0.000011f;
	PID.ek = 0.0f;
	PID.ek1 = 0.0f;
	PID.location_sum[0] = 0.0f;
	PID.location_sum[1] = 0.0f;
	PID.location_sum[2] = 0.0f;
	PID.location_sum[3] = 0.0f;
	PID.index = 0;
}

void PID_patroling(float actual_value, int8_t* Speed1, int8_t* Speed2)
{
	PID.ek = actual_value;
	PID.location_sum[PID.index] = actual_value;
	if(PID.index == 3)
	{
		PID.index = 0;
	}
	else
	{
		PID.index++;
	}
	float location_sum = (PID.location_sum[0] + PID.location_sum[1] + PID.location_sum[2] + PID.location_sum[3]) / 4.0f;
	float out = PID.kp * PID.ek + PID.ki * location_sum + PID.kd * (PID.ek - PID.ek1);
	if(out > 0)
	{
		if(*Speed2 < 90)
		{
			(*Speed2) += (int)out;
		}
		else if(*Speed1 > 80)
		{
			(*Speed1) -= (int)out;
		}
	}
	else if(out < 0)
	{
		if(*Speed1 < 90)
		{
			(*Speed1) += -(int)out;
		}
		else if(*Speed2 > 80)
		{
			(*Speed2) -= (int)(-out);
		}
	}
	//限定幅度为[30, 50]
	if (*Speed1 > 90)
	{
		*Speed1 = 90;
	}
	if (*Speed1 < 80)
	{
		*Speed1 = 80;
	}
	if (*Speed2 > 90)
	{
		*Speed2 = 90;
	}
	if (*Speed2 < 80)
	{
		*Speed2 = 80;
	}
	PID.ek1 = PID.ek;
}
